#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>

#include <opencv2/opencv.hpp>

#include <sstream>
#include <ctime>
#include <sstream>
#include <flashai/commons/string_utils.h>
#include "flashros/commons/utils.h"
#include "flashai/commons/string_utils.h"

using namespace flashai;
using namespace utils;

void CameraSubscribeCallback(const sensor_msgs::ImageConstPtr& msg) {
  try {
    auto img = cv_bridge::toCvShare(msg, "bgr8")->image;
    //cv::imshow("view", img);
    //cv::waitKey(1);


    std::stringstream ss;
    std::time_t t = std::time(NULL);
    std::tm tm = *std::localtime(&t);
    ss << std::put_time(&tm, "%Y%m%d%H%M%S");
    std::string img_path = "/home/robot/flashros/data/save_imgs_raw/" + ss.str() + ".jpg";
    cv::imwrite(img_path, img);


  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char** argv) {
  // ros init
  flashros::commons::RosInit(argc, argv, "FlashROS_Camera_Subscriber");
  ros::NodeHandle nh;

  // Read config file
  INIReader* reader =
      flashros::commons::InitConfig(nh, "/single_camera_publisher/config_filename");

  // create test window
  // cv::namedWindow("view");
  // cv::startWindowThread();

  // create subscribe and run
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub =
      it.subscribe(reader->GetString("output", "topic_name", ""), 1,
                   CameraSubscribeCallback);
  ros::spin();
  cv::destroyWindow("view");
}
